﻿// systetm
#include <math.h>
#include <float.h>
// event
#include "lane_detection.h"

int detect_current_lane(const RoadTopology* road, const Point2D* vehicle_pos)
{
	float cur_x = vehicle_pos->x;
	float cur_y = vehicle_pos->y;
	for (int i = 1; i <= road->count; i++)
	{
		LaneStrip* lanes = &road->lanes[i];
		int cur_y_ID = (int)cur_y;
		switch (road->insertDirection)
		{
		case INSERT_LEFT:
			if (cur_x > lanes->y2x[cur_y_ID] && cur_x < road->lanes[0].y2x[cur_y_ID])
			{
				return i;
			}
			break;
		case INSERT_RIGHT:
			if(cur_x < lanes->y2x[cur_y_ID] && cur_x > road->lanes[0].y2x[cur_y_ID])
			{
				return i;
			}
			break;
		default:break;
		}
		
	}
	return 0;
}

// 计算点到曲线的近似距离
//static float calc_approx_distance(const BezierCurve* curve, const Point2D* pt)
//{
//    float min_dist = FLT_MAX;
//
//    // 采样20个点找最近距离
//    for (int i = 0; i <= 20; ++i)
//    {
//        float t = i / 20.0f;
//        //Point2D p = bezier_calculate(curve, t);
//        float dx = p.x - pt->x;
//        float dy = p.y - pt->y;
//        float dist = dx * dx + dy * dy; // 平方距离
//
//        if (dist < min_dist)
//        {
//            min_dist = dist;
//        }
//    }
//    return sqrtf(min_dist);
//}
//int8_t detect_current_lane(const RoadTopology* road, const Point2D* vehicle_pos)
//{
//    int8_t result = -1;
//    float min_offset = FLT_MAX;
//
//    for (uint8_t i = 0; i < road->count; ++i) 
//    {
//        const LaneStrip* lane = &road->lanes[i];
//        float dist = calc_approx_distance(&lane->curve, vehicle_pos);
//
//        // 判断是否在车道右侧边界内
//        if (dist < lane->width && dist < min_offset) 
//        {
//            min_offset = dist;
//            result = i;
//        }
//    }
//    return result;
//////}